#!/usr/bin/env python3
# -*- coding: utf-8 -*-

"""
功能: 控制机械臂运动api接口函数
Api接口: 
        1. catch_sth()           机械臂手爪闭合
        2. release_sth()         机械臂手爪打开
        3. arm_catch_sth()       机械臂执行抓取动作  -50.0
        4. arm_normal_put_sth()  机械臂执行放置物体的动作 (不是堆叠放置的方式), 默认传入的高度为 -50
        5. arm_stack_put_sth()   机械臂放置堆叠物体时的动作组 (堆叠放置的方式), 默认传入的高度为  30
        6. arm_init_action()     机械臂初始化动作. 机械臂会从上一个角度转动到初始角度
        7. arm_move(target_x, target_y, target_z)   #机械臂从上一次的逆运动学位置，转动到新的目标位置 target_x, target_y, target_z

        8.  arm_move_forward(distance)   #机械臂向前运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
        9.  arm_move_back   (distance)   #机械臂向后运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
        10. arm_move_left   (distance)   #机械臂向左运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
        11. arm_move_right  (distance)   #机械臂向右运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
        12. arm_move_up     (distance)   #机械臂向上运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
        13. arm_move_down   (distance)   #机械臂向下运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
"""

import sys
import time
import math
from typing import Tuple
from . import user_arm
from . import servo_section
from . import serial_section
# import user_arm
# import servo_section
# import serial_section

class RobotArm:
    def __init__(self, auto_init=True):
        #创建并打开和下位机通信串口对象
        self.serial = serial_section.SerialPort(port="/dev/ttyS1", baudrate=115200)
        self.serial.open()
        time.sleep(1)
        #创建舵机协议对象
        self.servos = servo_section.ServoProtocol()
        time.sleep(1)
        #创建机械臂对象
        self.arm = user_arm.FourDofArm()

        #定义四自由度机械臂参数
        self.angle = [0.0, 0.0, 0.0, 0.0]
        self.start_ang = [50.0, 0.0, 0.0]  #机械臂逆运动学初始化参数
        self.save_ang = [50.0, 0.0, 0.0]   #实时保存四自由度机械臂逆运动学角度
        # self.start_ang = [126.0, 0.0, 109.0]  #机械臂逆运动学初始化参数
        # self.save_ang = [126.0, 0.0, 109.0]   #实时保存四自由度机械臂逆运动学角度
        for i in range(3):
            self.save_ang[i] = self.start_ang[i]


        self.arm_limit_x_min = 50
        self.arm_limit_x_max = 180
        self.arm_limit_y_min = -80
        self.arm_limit_y_max = 80
        self.arm_limit_z_min = -50
        self.arm_limit_z_max = 140
        
        # 可选的机械臂初始化动作
        if auto_init:
            # 机械臂执行初始动作
            self.release_sth() #打开爪子
            time.sleep(0.6)
            #机械臂执行初始化动作
            self.arm_init_action()
            time.sleep(0.6)


    def color_text(self, text, color=None, bg_color=None, bold=False, underline=False):
        codes = []
        if color:
            color_codes = {
                "black": 30, "red": 31, "green": 32, "yellow": 33,
                "blue": 34, "magenta": 35, "cyan": 36, "white": 37,
            }
            codes.append(color_codes.get(color, 39))
        if bg_color:
            bg_codes = {
                "black": 40, "red": 41, "green": 42, "yellow": 43,
                "blue": 44, "magenta": 45, "cyan": 46, "white": 47,
            }
            codes.append(bg_codes.get(bg_color, 49))
        if bold:
            codes.append(1)
        if underline:
            codes.append(4)
        
        if codes:
            return f"\033[{';'.join(map(str, codes))}m{text}\033[0m"
        else:
            return text

    def arm_limit_x(self, input_value_x):
        if input_value_x <= self.arm_limit_x_min:
            print(self.color_text("机械臂x轴到达最小值", color="red", bold=True))
            input_value_x = self.arm_limit_x_min
        if input_value_x >= self.arm_limit_x_max:
            print(self.color_text("机械臂x轴到达最大值", color="red", bold=True))
            input_value_x = self.arm_limit_x_max
        return input_value_x

    def arm_limit_y(self, input_value_y):
        if input_value_y <= self.arm_limit_y_min:
            print(self.color_text("机械臂y轴到达最小值", color="red", bold=True))
            input_value_y = self.arm_limit_y_min
        if input_value_y >= self.arm_limit_y_max:
            print(self.color_text("机械臂y轴到达最大值", color="red", bold=True))
            input_value_y = self.arm_limit_y_max
        return input_value_y

    def validate_position_strict(self, x: float, y: float, z: float) -> tuple[bool, str]:
        """
        严格验证机械臂位置是否在限位范围内
        
        Args:
            x: X坐标 (毫米)
            y: Y坐标 (毫米) 
            z: Z坐标 (毫米)
            
        Returns:
            tuple: (是否有效, 错误信息)
        """
        if not (self.arm_limit_x_min <= x <= self.arm_limit_x_max):
            return False, f"机械臂X坐标超出限位范围 {x}mm (有效范围: {self.arm_limit_x_min}-{self.arm_limit_x_max}mm)"
        if not (self.arm_limit_y_min <= y <= self.arm_limit_y_max):
            return False, f"机械臂Y坐标超出限位范围 {y}mm (有效范围: {self.arm_limit_y_min}-{self.arm_limit_y_max}mm)"
        if not (self.arm_limit_z_min <= z <= self.arm_limit_z_max):
            return False, f"机械臂Z坐标超出限位范围 {z}mm (有效范围: {self.arm_limit_z_min}-{self.arm_limit_z_max}mm)"
        return True, ""

    def get_current_position(self) -> Tuple[float, float, float]:
        """
        获取机械臂当前位置
        
        Returns:
            Tuple[float, float, float]: (x, y, z) 当前位置坐标 (毫米)
        """
        return self.save_ang[0], self.save_ang[1], self.save_ang[2]

    def arm_limit_z(self, input_value_z):
        if input_value_z <= self.arm_limit_z_min:
            print(self.color_text("机械臂z轴到达最小值", color="red", bold=True))
            input_value_z = self.arm_limit_z_min
        if input_value_z >= self.arm_limit_z_max:
            print(self.color_text("机械臂z轴到达最大值", color="red", bold=True))
            input_value_z = self.arm_limit_z_max
        return input_value_z

    """
    通过线性方程将物体坐标转换为机械臂抓取的逆运动学坐标
    """
    def calculate_arm_pos(self, x, y):
        position_x = 0
        position_y = 0

        if x <= 0:
            position_x = (0.821 * y + 114.2) - 10
            position_y = (0.3638 * x - 15.049) -6
        else:
            position_x = 0.821 * y + 114.2
            position_y = 0.3638 * x - 15.049 
            # position_y = (0.3638 * x - 15.049) + 8
        position_x = round(position_x, 2)
        position_y = round(position_y, 2)
        # print("position_x:", position_x, "position_y:", position_y)

        position_x = self.arm_limit_x(position_x)
        position_y = self.arm_limit_y(position_y)
        print(self.color_text(position_x, color="red", bold=True))
        print(self.color_text(position_y, color="red", bold=True))
        return position_x, position_y


    """
    机械臂手爪闭合
    """
    def catch_sth(self):
        data = self.servos.servo_control(
            servo_num=1,
            servo_id=[6],
            servo_angle=[110]
        )
        self.serial.serial_send(data)
        time.sleep(0.05)


    """
    机械臂手爪打开
    """
    def release_sth(self):
        data = self.servos.servo_control(
            servo_num=1,
            servo_id=[6],
            servo_angle=[1.01]
        )
        self.serial.serial_send(data)
        time.sleep(0.05)


    """
    机械臂移动函数
    该函数只需要传入机械臂逆运动学的目标位置即可.
    意思就是: 该函数会自动记录机械臂上一次机械臂的目标位置, 
            传入目标位置后, 机械臂会自动从上一次位置运动到新传入的目标位置
    """
    def arm_move(self, target_x, target_y, target_z):
        current_angle = [0.0, 0.0, 0.0]
        
        # 严格限位检查 - 拒绝执行超限命令
        is_valid, error_msg = self.validate_position_strict(target_x, target_y, target_z)
        if not is_valid:
            print(self.color_text(f"限位错误：{error_msg}", color="red", bold=True))
            return False
        
        delta_x = self.save_ang[0] - target_x
        delta_y = self.save_ang[1] - target_y
        num_points = 0
        #防止数据出错
        try:
            num_points = int((math.fabs(delta_x) + math.fabs(delta_y)) / 9.0)
        except:
            num_points = 1
        if num_points <= 8:
            num_points = 8
        if num_points >= 25:
            num_points = 25
        for i in range(num_points + 1):
            t = i / num_points
            x = self.save_ang[0] + t * (target_x - self.save_ang[0])
            y = self.save_ang[1] + t * (target_y - self.save_ang[1])
            z = self.save_ang[2] + t * (target_z - self.save_ang[2])
            x = self.arm_limit_x(x)
            y = self.arm_limit_y(y)
            z = self.arm_limit_z(z)

            current_angle[0] = x
            current_angle[1] = y
            current_angle[2] = z

            self.angle[0],self.angle[1],self.angle[2],self.angle[3] =  self.arm.get_coordinate(x, y, z)
            data = self.servos.servo_control(
                servo_num=4,
                servo_id=[44, 45, 46, 7],
                servo_angle=[self.angle[3], self.angle[2], self.angle[1], self.angle[0]]
            )
            self.serial.serial_send(data)
            time.sleep(0.03)
        self.save_ang = [current_angle[0], current_angle[1], current_angle[2]]
        return True


    """
    机械臂运动函数
    """
    def arm_move_to_position(self, start_x, start_y, start_z, end_x, end_y, end_z):
        save_x = 0
        save_y = 0
        save_z = 0
        delta_x = start_x - end_x
        delta_y = start_y - end_y
        #防止数据出错
        try:
            num_points = int((math.fabs(delta_x) + math.fabs(delta_y)) / 9.0)
        except:
            num_points = 1
        if num_points <= 8:
            num_points = 8
        if num_points >= 25:
            num_points = 25
        for i in range(num_points + 1):
            t = i / num_points
            x = start_x + t * (end_x - start_x)
            y = start_y + t * (end_y - start_y)
            z = start_z + t * (end_z - start_z)
            x = self.arm_limit_x(x)
            y = self.arm_limit_y(y)
            z = self.arm_limit_z(z)
            save_x = x
            save_y = y
            save_z = z
            self.angle[0],self.angle[1],self.angle[2],self.angle[3] =  self.arm.get_coordinate(x, y, z)
            
            data = self.servos.servo_control(
                servo_num=4,
                servo_id=[44, 45, 46, 7],
                servo_angle=[self.angle[3], self.angle[2], self.angle[1], self.angle[0]]
            )
            self.serial.serial_send(data)
            time.sleep(0.03)
        print("servo_angle:", self.angle[3], self.angle[2], self.angle[1], self.angle[0])
        self.save_ang = [save_x, save_y, save_z]


    """
    机械臂向前运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
    """
    def arm_move_forward(self, distance):
        current_angle = [0.0, 0.0, 0.0]
        delta_x = distance * 10
        end_x = self.save_ang[0] + distance * 10
        
        # 严格限位检查 - 拒绝执行超限命令
        is_valid, error_msg = self.validate_position_strict(end_x, self.save_ang[1], self.save_ang[2])
        if not is_valid:
            print(self.color_text(f"向前移动限位错误：{error_msg}", color="red", bold=True))
            return False
        
        num_points = 0
        #防止数据出错
        try:
            num_points = int(math.fabs(delta_x) / 9.0)
        except:
            num_points = 1
        if num_points <= 8:
            num_points = 8
        if num_points >= 25:
            num_points = 25
        for i in range(num_points + 1):
            t = i / num_points
            x = self.save_ang[0] + t * (end_x - self.save_ang[0])
            x = self.arm_limit_x(x)
            current_angle[0] = x

            self.angle[0],self.angle[1],self.angle[2],self.angle[3] =  self.arm.get_coordinate(x, self.save_ang[1], self.save_ang[2])
            data = self.servos.servo_control(
                servo_num=4,
                servo_id=[44, 45, 46, 7],
                servo_angle=[self.angle[3], self.angle[2], self.angle[1], self.angle[0]]
            )
            self.serial.serial_send(data)
            time.sleep(0.03)
        self.save_ang[0] = current_angle[0]
        return True


    """
    机械臂向后运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
    """
    def arm_move_back(self, distance):
        current_angle = [0.0, 0.0, 0.0]
        delta_x = distance * 10
        end_x = self.save_ang[0] - distance * 10
        
        # 严格限位检查 - 拒绝执行超限命令
        is_valid, error_msg = self.validate_position_strict(end_x, self.save_ang[1], self.save_ang[2])
        if not is_valid:
            print(self.color_text(f"向后移动限位错误：{error_msg}", color="red", bold=True))
            return False
        
        num_points = 0
        #防止数据出错
        try:
            num_points = int(math.fabs(delta_x) / 9.0)
        except:
            num_points = 1
        if num_points <= 8:
            num_points = 8
        if num_points >= 25:
            num_points = 25
        for i in range(num_points + 1):
            t = i / num_points
            x = self.save_ang[0] + t * (end_x - self.save_ang[0])
            x = self.arm_limit_x(x)
            current_angle[0] = x

            self.angle[0],self.angle[1],self.angle[2],self.angle[3] =  self.arm.get_coordinate(x, self.save_ang[1], self.save_ang[2])
            data = self.servos.servo_control(
                servo_num=4,
                servo_id=[44, 45, 46, 7],
                servo_angle=[self.angle[3], self.angle[2], self.angle[1], self.angle[0]]
            )
            self.serial.serial_send(data)
            time.sleep(0.03)
        self.save_ang[0] = current_angle[0]
        return True


    """
    机械臂向左运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
    """
    def arm_move_left(self, distance):
        current_angle = [0.0, 0.0, 0.0]
        delta_x = distance * 10
        end_x = self.save_ang[1] - distance * 10
        
        # 严格限位检查 - 拒绝执行超限命令
        is_valid, error_msg = self.validate_position_strict(self.save_ang[0], end_x, self.save_ang[2])
        if not is_valid:
            print(self.color_text(f"向左移动限位错误：{error_msg}", color="red", bold=True))
            return False
        
        num_points = 0
        #防止数据出错
        try:
            num_points = int(math.fabs(delta_x) / 9.0)
        except:
            num_points = 1
        if num_points <= 8:
            num_points = 8
        if num_points >= 25:
            num_points = 25
        for i in range(num_points + 1):
            t = i / num_points
            y = self.save_ang[1] + t * (end_x - self.save_ang[1])
            y = self.arm_limit_y(y)
            current_angle[1] = y

            self.angle[0],self.angle[1],self.angle[2],self.angle[3] =  self.arm.get_coordinate(self.save_ang[0], y, self.save_ang[2])
            data = self.servos.servo_control(
                servo_num=4,
                servo_id=[44, 45, 46, 7],
                servo_angle=[self.angle[3], self.angle[2], self.angle[1], self.angle[0]]
            )
            self.serial.serial_send(data)
            time.sleep(0.03)
        self.save_ang[1] = current_angle[1]
        return True


    """
    机械臂向右运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
    """
    def arm_move_right(self, distance):
        current_angle = [0.0, 0.0, 0.0]
        delta_x = distance * 10
        end_x = self.save_ang[1] + distance * 10
        
        # 严格限位检查 - 拒绝执行超限命令
        is_valid, error_msg = self.validate_position_strict(self.save_ang[0], end_x, self.save_ang[2])
        if not is_valid:
            print(self.color_text(f"向右移动限位错误：{error_msg}", color="red", bold=True))
            return False
        
        num_points = 0
        #防止数据出错
        try:
            num_points = int(math.fabs(delta_x) / 9.0)
        except:
            num_points = 1
        if num_points <= 8:
            num_points = 8
        if num_points >= 25:
            num_points = 25
        for i in range(num_points + 1):
            t = i / num_points
            y = self.save_ang[1] + t * (end_x - self.save_ang[1])
            y = self.arm_limit_y(y)
            current_angle[1] = y

            self.angle[0],self.angle[1],self.angle[2],self.angle[3] =  self.arm.get_coordinate(self.save_ang[0], y, self.save_ang[2])
            data = self.servos.servo_control(
                servo_num=4,
                servo_id=[44, 45, 46, 7],
                servo_angle=[self.angle[3], self.angle[2], self.angle[1], self.angle[0]]
            )
            self.serial.serial_send(data)
            time.sleep(0.03)
        self.save_ang[1] = current_angle[1]
        return True


    """
    机械臂向上运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
    """
    def arm_move_up(self, distance):
        current_angle = [0.0, 0.0, 0.0]
        delta_x = distance * 10
        end_x = self.save_ang[2] + distance * 10
        
        # 严格限位检查 - 拒绝执行超限命令
        is_valid, error_msg = self.validate_position_strict(self.save_ang[0], self.save_ang[1], end_x)
        if not is_valid:
            print(self.color_text(f"向上移动限位错误：{error_msg}", color="red", bold=True))
            return False
        
        num_points = 0
        #防止数据出错
        try:
            num_points = int(math.fabs(delta_x) / 9.0)
        except:
            num_points = 1
        if num_points <= 8:
            num_points = 8
        if num_points >= 25:
            num_points = 25
        for i in range(num_points + 1):
            t = i / num_points
            z = self.save_ang[2] + t * (end_x - self.save_ang[2])
            z = self.arm_limit_z(z)
            current_angle[2] = z

            self.angle[0],self.angle[1],self.angle[2],self.angle[3] =  self.arm.get_coordinate(self.save_ang[0], self.save_ang[1], z)
            data = self.servos.servo_control(
                servo_num=4,
                servo_id=[44, 45, 46, 7],
                servo_angle=[self.angle[3], self.angle[2], self.angle[1], self.angle[0]]
            )
            self.serial.serial_send(data)
            time.sleep(0.03)
        self.save_ang[2] = current_angle[2]
        return True


    """
    机械臂向上运动多少距离，单位：厘米（注意：距离为正数，程序会自动计算+-号）
    """
    def arm_move_down(self, distance):
        current_angle = [0.0, 0.0, 0.0]
        delta_x = distance * 10
        end_x = self.save_ang[2] - distance * 10
        
        # 严格限位检查 - 拒绝执行超限命令
        is_valid, error_msg = self.validate_position_strict(self.save_ang[0], self.save_ang[1], end_x)
        if not is_valid:
            print(self.color_text(f"向下移动限位错误：{error_msg}", color="red", bold=True))
            return False
        
        num_points = 0
        #防止数据出错
        try:
            num_points = int(math.fabs(delta_x) / 9.0)
        except:
            num_points = 1
        if num_points <= 8:
            num_points = 8
        if num_points >= 25:
            num_points = 25
        for i in range(num_points + 1):
            t = i / num_points
            z = self.save_ang[2] + t * (end_x - self.save_ang[2])
            z = self.arm_limit_z(z)
            current_angle[2] = z

            self.angle[0],self.angle[1],self.angle[2],self.angle[3] =  self.arm.get_coordinate(self.save_ang[0], self.save_ang[1], z)
            data = self.servos.servo_control(
                servo_num=4,
                servo_id=[44, 45, 46, 7],
                servo_angle=[self.angle[3], self.angle[2], self.angle[1], self.angle[0]]
            )
            self.serial.serial_send(data)
            time.sleep(0.03)
        self.save_ang[2] = current_angle[2]
        return True


    """
    机械臂初始化动作
    机械臂从上一次的角度转动到初始化动作
    """
    def arm_init_action(self):
        self.release_sth() #打开爪子
        time.sleep(0.6)
        self.arm_move_to_position(self.save_ang[0], self.save_ang[1], self.save_ang[2], self.start_ang[0], self.start_ang[1], self.start_ang[2])
        time.sleep(0.6)


    """
    机械臂执行抓取动作
    sth_position_x: 传入视觉检测到的待测物体的 x 坐标
    sth_position_y: 传入视觉检测到的待测物体的 y 坐标
    sth_position_z: 传入需要抓取物体的高度距离。
                    如果抓取的物体没有堆叠的情况, 那么该参数默认为-50.0
    """
    def arm_catch_sth(self, sth_position_x, sth_position_y, sth_position_z):
        x, y = self.calculate_arm_pos(sth_position_x, sth_position_y)
        #机械臂稍微向上抬升
        self.arm_move_to_position(self.start_ang[0], self.start_ang[1], self.start_ang[2], self.start_ang[0], self.start_ang[1], 50)
        time.sleep(0.6)

        # 机械臂转动到抓取位置
        self.arm_move_to_position(self.start_ang[0], self.start_ang[1], 50, x, y, 50)
        time.sleep(0.6)

        # 机械臂z轴竖直向下，准备抓取物体
        self.arm_move_to_position(x, y, 50.0, x, y, 0)
        self.arm_move_to_position(x, y, 0.0, x, y, sth_position_z)
        time.sleep(0.6)

        # 机械臂手爪闭合，抓取物体
        self.catch_sth()
        time.sleep(0.6)
        # 机械臂抬升，准备离开抓取区域
        self.arm_move_to_position(x, y, sth_position_z, x, y, 0.0)
        time.sleep(0.6)
        # 机械臂转动到初始位置
        self.arm_move_to_position(x, y, 0.0, self.start_ang[0], self.start_ang[1], self.start_ang[2])
        time.sleep(0.6)


    """
    机械臂执行放置物体的动作（不是堆叠放置的方式）
    sth_position_x: 传入物体需要放置到图像中的 x 坐标
    sth_position_y: 传入物体需要放置到图像中的 y 坐标
    sth_position_z: 传入物体需要放置到图像中的 z 高度
    哈哈哈哈哈, 通过多次测试, 针对目前的物体, 放置高度设置为-50就好了
    注意, 注意, 注意, 通过多次测试, 针对目前的物体, 放置高度设置为-50就好了
    """
    def arm_normal_put_sth(self, sth_position_x, sth_position_y, sth_position_z):
        x, y = self.calculate_arm_pos(sth_position_x, sth_position_y)
        print("x:", x, "y:", y)

        # #机械臂稍微向上抬升，防止机械臂碰到其他障碍物
        self.arm_move_to_position(self.start_ang[0], self.start_ang[1], self.start_ang[2], self.start_ang[0], self.start_ang[1], 50)
        time.sleep(0.6)

        # 机械臂转动到放置位置
        # self.arm_move_to_position(self.start_ang[0], self.start_ang[1], 50, x, y, 50)
        self.arm_move_to_position(self.start_ang[0], self.start_ang[1], 50, x, y, 50)
        time.sleep(0.6)

        # 机械臂下降，准备释放物体
        self.arm_move_to_position(x, y, 50, x, y, 0)
        # self.arm_move_to_position(x, y, 0, x, y, -50)
        print("-----------input_z:-----------", sth_position_z)
        self.arm_move_to_position(x, y, 0, x, y, sth_position_z)
        time.sleep(0.6)

        # 机械臂手爪闭合，抓取物体
        self.release_sth()
        time.sleep(0.6)

        # 机械臂抬升，准备离开放置区域
        # self.arm_move_to_position(x, y, -50, x, y, 0.0)
        self.arm_move_to_position(x, y, sth_position_z, x, y, 0.0)
        time.sleep(0.6)

        # 机械臂转动到初始位置
        self.arm_move_to_position(x, y, 0, self.start_ang[0], self.start_ang[1], self.start_ang[2])
        time.sleep(0.6)


    """
    机械臂放置堆叠物体时的动作组（堆叠放置的方式）
    sth_position_x: 传入物体需要放置到图像中的 x 坐标
    sth_position_y: 传入物体需要放置到图像中的 y 坐标
    sth_position_z: 传入物体需要放置到图像中的 z 高度
    哈哈哈哈哈, 通过多次测试, 针对目前的物体, 放置高度设置为 30 就好了。
    注意, 注意, 注意, 通过多次测试, 针对目前的物体, 放置高度设置为 30 就好了
    """
    def arm_stack_put_sth(self, sth_position_x, sth_position_y, sth_position_z):
        x, y = self.calculate_arm_pos(sth_position_x, sth_position_y)

        # self.arm_move_to_position(self.start_ang[0], self.start_ang[1], self.start_ang[2], self.start_ang[0], self.start_ang[1], 30)
        self.arm_move_to_position(self.start_ang[0], self.start_ang[1], self.start_ang[2], self.start_ang[0], self.start_ang[1], 40)
        time.sleep(0.6)

        # 机械臂转动到放置位置
        # self.arm_move_to_position(self.start_ang[0], self.start_ang[1], 30, x, y, 30)
        self.arm_move_to_position(self.start_ang[0], self.start_ang[1], 40, x, y, 40)
        time.sleep(0.6)

        # 机械臂手爪闭合，抓取物体
        self.release_sth()
        time.sleep(0.6)

        # 机械臂抬升，准备离开放置区域
        # self.arm_move_to_position(x, y, 30, x, y, 76.0)
        self.arm_move_to_position(x, y, 40, x, y, 76.0)
        time.sleep(0.6)

        self.arm_move_to_position(x, y, 76, self.start_ang[0], self.start_ang[1], 76.0)
        time.sleep(0.6)

        # 机械臂转动到初始位置
        self.arm_move_to_position(self.start_ang[0], self.start_ang[1], 76.0, self.start_ang[0], self.start_ang[1], self.start_ang[2])
        time.sleep(0.6)


def main():
    arm = RobotArm()
    #机械臂执行初始化动作-动作测试成功
    # arm.arm_init_action()
    # time.sleep(0.6)
    # arm.catch_sth()
    # time.sleep(1)
    # arm.arm_normal_put_sth(37, -109, 50)

    # arm.arm_move_forward(1)  #机械臂向前1厘米--测试成功
    # time.sleep(1)
    # arm.arm_move_forward(3)  #机械臂向前3厘米--测试成功
    # time.sleep(1)
    # arm.arm_move_back(3)     #机械臂向后3厘米--测试成功
    # time.sleep(1)
    # arm.arm_move_back(1)     #机械臂向后1厘米--测试成功
    # time.sleep(1)
    # arm.arm_move_left(1)     #机械臂向左1厘米--测试成功
    # time.sleep(1)
    # arm.arm_move_left(3)     #机械臂向左3厘米--测试成功
    # time.sleep(1)
    # arm.arm_move_right(3)    #机械臂向右3厘米--测试成功
    # time.sleep(1)
    # arm.arm_move_right(1)    #机械臂向右1厘米--测试成功
    # time.sleep(1)


    # arm.arm_move_up(1)       #机械臂向上1厘米--测试成功
    # time.sleep(1)
    # arm.arm_move_up(3)       #机械臂向上3厘米--测试成功
    # time.sleep(1)
    # arm.arm_move_down(3)     #机械臂向下3厘米--测试成功
    # time.sleep(1)
    # arm.arm_move_down(1)     #机械臂向下1厘米--测试成功
    # time.sleep(1)

if __name__ == '__main__':
    main()